﻿using System;
using Microsoft.Xna.Framework;

namespace PoolGame2.Physics
{

    public enum BallType
    {
        CUE_BALL, STRIPPED_BALL, SPOT_BALL, BLACK_BALL
    }
    
    public class Ball : PhysicsObject
    {
        private Vector3 previousPosition;
        private Matrix orientation;
        
        public Vector3 angularVelocity;
        
        // known as r in the Strike fuction
        private Vector3 initialStrikeOffset;

        private BallType ballType;
        private int ballNumber;

        private bool pocketed;

        public Pocket endPocket;
        

        public Ball(BallType type, int num, float r) {
            this.ballNumber = num;
            this.ballType = type;
            this.orientation = Matrix.Identity;
            this.endPocket = null;
            this.pocketed = false;
        }

        public void setOrientation(Matrix o) {
            this.orientation = o;
        }

        public void setPocketed(bool val)
        {
            this.pocketed = val;
        }

        public void setPocketed()
        {
            this.pocketed = true;
        }

        public bool getPocketed()
        {
            return pocketed;
        }

        public int getNumber()
        {
            return ballNumber;
        }

        public Matrix getOrientation()
        {
            return orientation;
        }

        public BallType GetBallType()
        {
            return ballType;
        }

        public Collision CollidingWith(Cushion cushion, float time)
        {
            //float collisionTime1 = TestCushion(cushion.start, cushion.mainStart, cushion.startNormal, time);
            float collisionTime2 = TestCushion(cushion.mainStart, cushion.mainEnd, cushion.mainNormal, time);
            //float collisionTime3 = TestCushion(cushion.mainEnd, cushion.end, cushion.endNormal, time);

            //if (collisionTime1 >= 0)
            //{
            //    return new Collision(this, cushion, collisionTime2, 0);
            //}

            if (collisionTime2 >= 0)
            {
                return new BallCushionCollision(this, cushion, collisionTime2, 1);
            }
            
            //if (collisionTime3 >= 0)
            //{
            //    return new Collision(this, cushion, collisionTime2, 2);
            //}
            return null;
       }

        private float TestCushion(Vector3 start, Vector3 end, Vector3 normal, float time)
        {
            Vector3 ballStartPos = this.getPosition();
            Vector3 ballEndPos = this.getPosition() + (this.getVelocity() * time);

            // the intesection line
            Vector3 startPointOnLine = start + (normal * PhysicsConstants.DEFAULT_RADIUS);
            Vector3 endPointOnLine = end + (normal * PhysicsConstants.DEFAULT_RADIUS);

            float cushStartDot = Vector3.Dot(normal, ballStartPos - startPointOnLine);
            float cushEndDot = Vector3.Dot(normal, ballEndPos - startPointOnLine);

            if ((cushStartDot > 0 && cushEndDot < 0) || (cushStartDot < 0 && cushEndDot > 0))
            {
                // We have a collision
                
                // check if the ball was moving towards or away from the cushion. If it is away that is just collision that has been resolved
                if (Vector3.Dot(this.velocity, normal) < 0)
                {
                    //calculate the time of the collision

                    // This is a complete shortcut - will put the proper implementation in later
                    float collisionTimeX = 0;
                    float s = ballStartPos.X;
                    float e = ballEndPos.X;
                    float m = startPointOnLine.X;
                    collisionTimeX = (float)(Math.Abs(s - m) / Math.Abs(s - e));
                    s = ballStartPos.Z;
                    e = ballEndPos.Z;
                    m = startPointOnLine.Z;
                    float collisionTimeZ = (float)(Math.Abs(s - m) / Math.Abs(s - e));

                    float earliest = (collisionTimeX < collisionTimeZ) ? collisionTimeX : collisionTimeZ;

                    //if (earliest > 0 && earliest < 1)
                    //{
                        // final check - within the start end bounds
                        Vector3 collisionPoint = ballStartPos + (this.getVelocity() * time * earliest);

                        // again a shortcut - the cushions are more or less parrallel to the x and z axis - We will ignore the other part of the cushion for demo
                        if (collisionTimeZ < collisionTimeX)
                        {
                            float minX = start.X;
                            float maxX = end.X;

                            if (minX > maxX)
                            {
                                minX = end.X;
                                maxX = start.X;
                            }
                            if (collisionPoint.X > minX && collisionPoint.X < maxX) {
                                return collisionTimeZ * time;
                            }
                        }
                        else
                        {
                            float minZ = start.Z;
                            float maxZ = end.Z;

                            if (minZ > maxZ)
                            {
                                minZ = end.Z;
                                maxZ = start.Z;
                            }
                            if (collisionPoint.Z > minZ && collisionPoint.Z < maxZ) {
                                return collisionTimeX * time;
                            }
                        }
                    //}
                }
            }
            return -1;
        }

        /// <summary>
        /// Tests if this ball is colliding with another ball
        /// </summary>
        /// <param name="ball2"></param>
        /// <param name="time"></param>
        /// <returns></returns>
        public Collision CollidingWith(Ball ball2, float time)
        {
            Vector3 velocityDiff = this.getVelocity() - ball2.velocity;
            Vector3 positionDiff = position - ball2.position;
            Collision c = this.solveQuadratic(velocityDiff, positionDiff, ball2, time);
            return c;
        }

        /// <summary>
        ///  This Method needs to be updated to support timesteping
        /// </summary>
        /// <param name="pocket"></param>
        /// <param name="time"></param>
        /// <returns></returns>
        public Collision CollidingWith(Pocket pocket, float time)
        {
            Vector3 tmp = this.position;
            tmp.Y = pocket.getPosition().Y;
            //Think this was a bug - works properly when the radius is halved
            if (Vector3.Distance(tmp, pocket.getPosition()) < (PhysicsConstants.POCKET_RADIUS / 2))
            {
                // If they're colliding, then make the ball "fall"
                if (this.getPosition().Y > 40.0f && (this.getVelocity().X < 40.0f || this.getVelocity().Z < 40.0f))
                {
                    this.setPosition(new Vector3(pocket.getPosition().X, this.getPosition().Y - 1.0f, pocket.getPosition().Z));
                    }
                // Basic "if it's going to fast, pop it out" code
                else if (this.getVelocity().X > 40.0f || this.getVelocity().Z > 40.0f)
                {
                    this.setVelocity(new Vector3(-this.getVelocity().X, 0.0f, -this.getVelocity().Z));
                }
                else if (this.getPosition().Y <= 40.0f)
                {
                    this.setPocketed();
                    endPocket = pocket;
                    this.setVelocity(new Vector3(0.0f, 0.0f, 0.0f));
                }
                //TODO: Shortcut here
                return new BallPocketCollision(this, pocket, 0.0f);
            }
            return null;
        }

        /// <summary>
        /// The main integration call
        /// </summary>
        /// <param name="time">The amount of time to integrate forward by</param>
        /// <param name="forTracking">If this integration is for shot tracking purposes</param>
        public override void Integrate(float time, bool forTracking)
        {
            this.previousPosition = position;
            float len = velocity.Length();
            if (len > 0.0f && len < PhysicsConstants.MIN_VELOCITY)
            {
                velocity = Vector3.Zero;
            }
            else if (len != 0.0f) 
            {
                //base.Integrate(time);
                Vector3 perimeterVelocity = Vector3.Cross(angularVelocity, PhysicsConstants.INTEGRATE_R) + velocity;
                Vector3 normalizedPerimeterVelocity = Vector3.Normalize(perimeterVelocity);
                
                // integrate for linear velocity
                Vector3 velocityChange = PhysicsConstants.INTEGRATE_FRICTION_BY_GRAVITY * normalizedPerimeterVelocity * time;
                
                // integrate for angular velocity
                Vector3 omegaChange = PhysicsConstants.INTEGRATE_OMEGA_MULTIPLYER * normalizedPerimeterVelocity;
                omegaChange = Vector3.Cross(initialStrikeOffset, omegaChange);
                omegaChange = omegaChange * PhysicsConstants.INVERSE_MOMENT_OF_INTERIA * time;

                velocity += velocityChange;
                angularVelocity += omegaChange;

                Vector3 mul = Vector3.Multiply(velocity, time);
                position = Vector3.Add(position, mul);

                // calculate the distance travelled
                float dist = Vector3.DistanceSquared(previousPosition, position);
                if (dist > 0)
                {
                    // calculate the direction vector
                    Vector3 direction = previousPosition - position;

                    // cross product this to get the correct direction
                    direction = Vector3.Cross(direction, Vector3.Up);
                    direction.Normalize();

                    
                    if (!forTracking)
                    {
                        // calculate the roll
                        float angle = dist / PhysicsConstants.DEFAULT_CIRCUMFERENCE / 2;

                        // calculate the change in orientation
                        Matrix mat = Matrix.CreateFromAxisAngle(direction, angle);
                        orientation *= mat;
                    }
                }
            }
        }

        private Collision solveQuadratic(Vector3 velocityDiff, Vector3 positionDiff, Ball ball, float time)
        {
            float a = Vector3.Dot(velocityDiff, velocityDiff);
            if (a != 0.0f)
            {
                float b = Vector3.Dot(positionDiff, velocityDiff) / a;
                float c = (Vector3.Dot(positionDiff, positionDiff) - 4 * PhysicsConstants.DEFAULT_RADIUS * PhysicsConstants.DEFAULT_RADIUS) / a;
                c = (b * b > c) ? (float)(Math.Sqrt(b * b - c)) : 1000f;

                float t1 = -b + c;
                float t2 = -b - c;

                if ((t1 <= 0 || t1 >= time) && (t2 <= 0 || t2 >= time))
                {
                    return null;
                }
                return new BallBallCollision(this, ball, Math.Min(t1, t2));
            }
            return null;
        }

        /**
         * Calculates an initial velocity for the ball as if its being struke by a cue, using hooks law
         * Assumes that the ball being stuck is stationary.
         */
        public void Strike(Vector3 x, Vector3 r)
        {
            velocity = (-PhysicsConstants.SPRING_CONSTANT * PhysicsConstants.CUE_CONTACT_TIME / PhysicsConstants.MASS) * x;
            angularVelocity = PhysicsConstants.INVERSE_MOMENT_OF_INTERIA * PhysicsConstants.CUE_CONTACT_TIME * Vector3.Cross(r, (-PhysicsConstants.SPRING_CONSTANT * x));
            this.initialStrikeOffset = r;
        }

    }
}
